/**   
 * Copyright 2011 COMERE Harold
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *   http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package etrl.geometry;

import java.util.ArrayList;
import java.util.Iterator;

import etrl.camera.Camera;
import etrl.core.Geode;
import etrl.core.Group;
import etrl.core.Lights;
import etrl.core.Node;
import etrl.core.NodeVisitor;
import etrl.core.Transform;
import etrl.core.TransformData;
import etrl.vecmath.Matrix4f;
import etrl.vecmath.Vector3f;
import etrl.vecmath.Vector4f;

/**
 * \class LineSegmentIntersector
 * \brief Specialized NodeVisitor to find intersections between a segment and drawable geometries.
 * @author harold
 *
 */
public class LineSegmentIntersector implements NodeVisitor
{		
	/**
	 * \class Intersection
	 * \brief Storage class to keep intersection data.
	 * @author harold
	 *
	 */
	public class Intersection
	{
		/**
		 * \brief The intersection point coordinates in world space
		 */
		public Vector4f _worldIntersectionPoint;
		
		/**
		 * \brief The normal of the intersected triangle, exprimed in world space
		 */
		public Vector3f _worldIntersectedNormal;
		
		/**
		 * \brief The intersected drawable
		 */
		public Drawable _drawable;
	}
	
	/**
	 * \class IntersectionTriangleVisition
	 * \brief Specialized triangle visitor to check intersection between triangle and ray
	 * @author harold
	 *
	 */
	private class IntersectionTriangleVisition implements TriangleVisitor
	{
		/**
		 * Ray start point in drawable local space
		 */
		private Vector4f _start;
		
		/**
		 * Ray end point in drawable local space
		 */
		private Vector4f _end;
		
		/**
		 * Drawable model matrix 
		 */
		private Matrix4f _modelMatrix;
		
		/**
		 * Found intersections list
		 */
		private ArrayList <Intersection> _intersections;
		
		/**
		 * Current visited drawable
		 */
		private Drawable _drawable;
		
		/**
		 * \brief Initializes the visitor 
		 * @param[in] drawable Current visited drawable
		 * @param[in] start Ray start point in drawable local space
		 * @param[in] end Ray end point in drawable local space
		 * @param[in] modelMatrix Drawable model matrix
		 */
		public IntersectionTriangleVisition(Drawable drawable, Vector4f start, Vector4f end, Matrix4f modelMatrix)
		{
			_start = new Vector4f(start);
			_end = new Vector4f(end);
			_modelMatrix = new Matrix4f(modelMatrix);
			_intersections = new ArrayList <Intersection>();
			_drawable = drawable;
		}
		
		/**
		 * {@inheritDoc}
		 */
		
		public void visit(Vector4f A, Vector4f B, Vector4f C)
		{
			Vector4f intersectionPoint = new Vector4f();
			
			// Check if the ray intersects the givent triangle
			// If it intersects it
			if(intersectsTriangle(A, B, C, intersectionPoint) == 1)
			{
				// Transform the intersection point to world space
				_modelMatrix.transform(intersectionPoint);
				
				// And store the intersection data
				Intersection intersection = new Intersection();
				intersection._worldIntersectionPoint = intersectionPoint;
				intersection._drawable = _drawable;
				_intersections.add(intersection);
			}
		}
		
		/**
		 * \brief Gets the intersections list
		 * @return Intersection list.
		 */
		public ArrayList <Intersection> getIntersections()
		{
			return _intersections;
		}
	
		/**
		 * Epsilon value to deal with double precision
		 */
		private final double EPSILON = 0.00000001;
		
		/**
		 * \brief Checks if the ray intersects the given triangle
		 * @param[in] A First point of the triangle to intersect
		 * @param[in] B Second point of the triangle to intersect
		 * @param[in] C Third point of the triangle to intersect
		 * @param[out] I Found intersection point
		 * @return 	-1 	: triangle is degenerate (a segment or point)
		 * 			0	: disjoint (no intersect)
		 * 			1	: intersect in unique point I1
		 * 			2	: are in the same plane
		 */
		private int intersectsTriangle(Vector4f A, Vector4f B, Vector4f C, Vector4f I)
		{
			Vector3f 	u, v, n; 		// triangle vectors
		    Vector3f 	dir, w0, w;		// ray vectors
		    float  		r, a, b;    	// params to calc ray-plane intersect

		    // get triangle edge vectors and plane normal
		    u = new Vector3f(B.x, B.y, B.z);
		    u.sub(new Vector3f(A.x, A.y, A.z));
		    
		    v = new Vector3f(C.x, C.y, C.z);
		    v.sub(new Vector3f(A.x, A.y, A.z));
		    
		    // cross product
		    n = new Vector3f();
		    n.cross(u, v);
		    
		    if (n.x == 0.f && n.y == 0.f && n.z == 0.f)	// triangle is degenerate
		        return -1;                				// do not deal with this case

		    dir = new Vector3f(_end.x, _end.y, _end.z);			// ray direction vector
		    dir.sub(new Vector3f(_start.x, _start.y, _start.z));
		    
		    w0 = new Vector3f(_start.x, _start.y, _start.z);
		    w0.sub(new Vector3f(A.x, A.y, A.z));
		    
		    Vector3f adotVect = new Vector3f(n);
		    a = - adotVect.dot(w0);
		    
		    Vector3f bdotVect = new Vector3f(n);
		    b = bdotVect.dot(dir);

		    if (Math.abs(b) < EPSILON) 
		    {     							// ray is parallel to triangle plane
		        if (a == 0)                	// ray lies in triangle plane
		            return 2;
		        else return 0;           	// ray disjoint from plane
		    }

		    // get intersect point of ray with triangle plane
		    r = a / b;
		    if (r < 0.0)                   // ray goes away from triangle
		        return 0;                  // => no intersect
		    								// for a segment, also test if (r > 1.0) => no intersect

		    // intersect point of ray and plane
		    dir.scale(r);
		    Vector3f rb = new Vector3f(_start.x, _start.y, _start.z);
		    rb.add(dir);
		    
		    
		    I.x = rb.x;
		    I.y = rb.y;
		    I.z = rb.z;
		    I.w = 1.f;

		    // is I inside T?
		    float    uu, uv, vv, wu, wv, D;
		    uu = new Vector3f(u).dot(new Vector3f(u));
		    uv = new Vector3f(u).dot(new Vector3f(v));
		    vv = new Vector3f(v).dot(new Vector3f(v));
		   
		    w = new Vector3f(I.x, I.y, I.z); 
		    w.sub(new Vector3f(A.x, A.y, A.z));
		    
		    wu = new Vector3f(w).dot(new Vector3f(u));
		    wv = new Vector3f(w).dot(new Vector3f(v));
		    D = uv * uv - uu * vv;

		    // get and test parametric coords
		    float s, t;
		    s = (uv * wv - vv * wu) / D;
		    if (s < 0.0 || s > 1.0)        // I is outside T
		        return 0;
		    t = (uv * wu - uu * wv) / D;
		    if (t < 0.0 || (s + t) > 1.0)  // I is outside T
		        return 0;

		    return 1;                      // I is in T
		}

	}
	
	/**
	 * Ray start point in world space
	 */
	private Vector4f _worldStart;
	
	/**
	 * Ray end point in world space
	 */
	private Vector4f _worldEnd;
	 
	/**
	 * Ray start point computed in the current local space
	 */
	private Vector4f _localStart;
	
	/**
	 * Ray end point computed in the current local space
	 */	
	private Vector4f _localEnd;
	
	/**
	 * Stack of transformations found while traversing. Used to transform ray into current space
	 */
	private ArrayList<TransformData> _transformDataStack;
	
	/**
	 * Intersections stack
	 */
	private ArrayList <Intersection> _intersections;
	
	/**
	 * \brief Initializes the intersector
	 * @param start Ray start point
	 * @param end Ray end point
	 */
	public LineSegmentIntersector(Vector4f start, Vector4f end)
	{
		super();
		
		_worldStart = new Vector4f(start);
		_worldEnd = new Vector4f(end);
		_localStart = new Vector4f(start);
		_localEnd = new Vector4f(end);
		
		_transformDataStack = new ArrayList<TransformData>();
		_intersections = new ArrayList <Intersection>();
		
		// Initialize the transform data stack with identity transform
		TransformData identity = new TransformData();
		_transformDataStack.add(identity);
	}
	
	/**
	 * {@inheritDoc}
	 */
	
	public void visit(Node node)
	{
	}

	/**
	 * {@inheritDoc}
	 */
	
	public void visit(Geode geode)
	{
		// If the ray intersects the geode bounding sphere, compute intersection between ray and geometry
		if(geode.getBounds().intersectsRay(_localStart, _localEnd))
		{
			// On each drawable in the geode
			ArrayList <Drawable> drawables = geode.getDrawables();
			for(Iterator <Drawable> it = drawables.iterator(); it.hasNext();)
			{
				// Get the model matrix
				Matrix4f modelMatrix = _transformDataStack.get(_transformDataStack.size() - 1).toMat4();
				
				// Then launch the triangle visitor to get intersections with the drawable
				Drawable drawable = it.next();
				IntersectionTriangleVisition triangleVisitor = new IntersectionTriangleVisition(drawable, _localStart, _localEnd, modelMatrix);
				drawable.accept(triangleVisitor);
				
				// Then add the found intersections to global intersection stack
				ArrayList <Intersection> intersections = triangleVisitor.getIntersections();
				for(Iterator <Intersection> itinter = intersections.iterator(); itinter.hasNext();)
				{
					_intersections.add(itinter.next());
				}
			}
		}	
	}

	/**
	 * {@inheritDoc}
	 */
	
	public void visit(Group group)
	{
		// If the ray intersects the group bounding sphere, continue traversing, else it is not needed
		if(group.getBounds().intersectsRay(_localStart, _localEnd))
			group.traverse(this);
	}

	/**
	 * {@inheritDoc}
	 */
	
	public void visit(Transform transform)
	{	
		// Well the getBounds on a Transform will return the bounding box with the transformation applied on it.
		// So to do intersection test we must to not transform yet ray points
		// Then if it intersects, transform ray points and continue traversing childs
		if(transform.getBounds().intersectsRay(_localStart, _localEnd))
		{
			// The transform will be traversed so stack its transform
			TransformData transformData = _transformDataStack.get(_transformDataStack.size() - 1).clone();
			transformData.apply(transform.getData());
			_transformDataStack.add(transformData);
			
			// Transform the ray points
			computeLocalRay();
			
			// And traverse childs
			transform.traverse(this);
		
			// Then we can pop the transform data
			_transformDataStack.remove(_transformDataStack.size() - 1);
		
			// And recompute ray points
			computeLocalRay();
			
		}
	}

	/**
	 * {@inheritDoc}
	 */
	
	public void visit(Lights lights)
	{
		visit((Group) lights);
	}
	
	/**
	 * {@inheritDoc}
	 */
	
	public void visit(Camera camera)
	{
		visit((Group) camera);		
	}
	
	/**
	 * Computes the ray points coordinate into the current local space.
	 */
	private void computeLocalRay()
	{
		TransformData transformData = _transformDataStack.get(_transformDataStack.size() - 1).clone();
	
		// As the current local space has changed, we have to recompute the local ray start & end
		// The model matrix defined by the transform stack is go from model space to world space. 
		// In our case we want to go from world space to model space, so use inverse matrix
		Matrix4f inverseModelMatrix = transformData.toMat4();
		inverseModelMatrix.invert();
		
		_localStart = new Vector4f(_worldStart);
		inverseModelMatrix.transform(_localStart);
	
		_localEnd = new Vector4f(_worldEnd);
		inverseModelMatrix.transform(_localEnd);
	}
	
	/**
	 * Gets the found intersections list.
	 * @return Intersection list.
	 */
	public ArrayList <Intersection> getIntersections()
	{
		return _intersections;
	}
}
